#include <iostream>
#include <cmath>
#include <eigen3/core>
#include <eigen3/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char* argv[])
{
    Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,1).toRotationMatrix();  //角轴转换为旋转矩阵；
    
    Quaterniond q(R);   //根据旋转矩阵转换为旋转四元数；
    
    Sophus::SO3 SO3_R(R); //旋转矩阵构造SO3;
    
    Sophus::SO3 SO3_q(q); //四元数构建SO3;
    
    cout<<"SO(3) from Matrix3d:\n"<<SO3_R.matrix()<<endl;
    
    cout<<"SO(3) from Quaterniond:\n"<<SO3_q.matrix()<<endl;
    
    cout<<"they are equal"<<endl;
    
    Vector3d so3=SO3_R.log();  //对数映射得到李代数   R=exp（pha.hat）
    cout<<"so3= "<<Sophus::SO3d::hat(so3)<<endl; //从向量到反对称矩阵；
    
    cout<<"so3 hat vee = "<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;  //vee对矩阵求pha
    
    //  增量扰动模型
    Vector3d update_so3(1e-4,0,0);  //增量
    Sophus::SO3d_update = Sophus::SO3d::exp(update_so3)*SO3_R;
    cout<<"SO3 updated =\n"<<SO3_updated.matrix()<<endl;
    
    cout<<"*************************"<<endl;
    Vector3d t(1,0,0);
    Sophus::SE3d SE3_Rt(R,t);
    Sophus::SE3d SE3_qt(q,t);
    cout<<"SE3 from R,t = "<<SE3_Rt.matrix()<<endl;
    cout<<"SE3 from q,t = "<<SE3_qt.matrix()<<endl;
    
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3=SE3_Rt.log();
    cout<<"se3= "<<se3.transpose()<<endl;
    
    cout<<"se3 hat= \n"<<Sophus::SE3d::hat(se3)<<endl;
    cout<<"se3 vee= \n"<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;

    
    Vextor6d update_se3;
    update_se3.setZero();
    Sophus::SE3d SE3_update=Sophus::SE3d::exp(update_se3)*SE3_Rt;
    cout<<"SE3 update ="<<endl<<SE3_update.matrix()<<endl;
    
    return 0;
}
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    

    
    

